/*
 * as5600.c
 *
 *  Created on: [Your Date]
 *      Author: [Your Name]
 *
 *  Description:
 *  Implementation file for the AS5600 driver.
 *  To port this to another platform, you only need to change the implementation
 *  of the _AS5600_ReadReg and _AS5600_WriteReg functions.
 */

/*
as5600的使用方法:
1-初始化
if (AS5600_Init(&hi2c1) != HAL_OK) {
    while(1);
  }
2-获取角度
AS5600_GetScaledAngle(I2C_HandleTypeDef *hi2c,uint16_t *raw_angle)
*/


#include "component_as5600.h"

// I2C 超时时间
#define I2C_TIMEOUT 10 // ms

/*
 * 平台相关的底层 I2C 读写函数
 * 如果要移植到其他平台 (如 Arduino, ESP-IDF)，只需修改这两个函数
 */

/**
 * @brief 从 AS5600 的指定寄存器读取数据
 * @param hi2c I2C 句柄
 * @param regAddr 寄存器地址
 * @param pData 指向存储读取数据的缓冲区的指针
 * @param len 要读取的数据长度 (字节)
 * @return HAL 状态
 */
static HAL_StatusTypeDef _AS5600_ReadReg(I2C_HandleTypeDef *hi2c,
                                         uint8_t regAddr, uint8_t *pData,
                                         uint16_t len) {
  // 注意：HAL 库函数要求 I2C 地址左移一位
  return HAL_I2C_Mem_Read(hi2c, (AS5600_I2C_ADDR << 1), regAddr,
                          I2C_MEMADD_SIZE_8BIT, pData, len, I2C_TIMEOUT);
}

/**
 * @brief 向 AS5600 的指定寄存器写入数据
 * @param hi2c I2C 句柄
 * @param regAddr 寄存器地址
 * @param pData 指向要写入数据的缓冲区的指针
 * @param len 要写入的数据长度 (字节)
 * @return HAL 状态
 */
static HAL_StatusTypeDef __attribute__((unused)) _AS5600_WriteReg(I2C_HandleTypeDef *hi2c, uint8_t regAddr, uint8_t *pData,uint16_t len) {
  return HAL_I2C_Mem_Write(hi2c, (AS5600_I2C_ADDR << 1), regAddr,
                           I2C_MEMADD_SIZE_8BIT, pData, len, I2C_TIMEOUT);
}

/*
 * 公共 API 函数实现
 */

HAL_StatusTypeDef AS5600_Init(I2C_HandleTypeDef *hi2c) {
  // 检查设备是否就绪，可以尝试读取一个寄存器
  uint8_t status_reg;
  HAL_StatusTypeDef status = AS5600_GetStatus(hi2c, &status_reg);
  return status;
}

HAL_StatusTypeDef AS5600_GetRawAngle(I2C_HandleTypeDef *hi2c,
                                     uint16_t *raw_angle) {
  uint8_t buf[2];
  HAL_StatusTypeDef status =
      _AS5600_ReadReg(hi2c, AS5600_REG_RAW_ANGLE_H, buf, 2);

  if (status == HAL_OK) {
    // 12 位角度值存储在高字节和低字节中
    // 高字节是 [11:8]，低字节是 [7:0]
    *raw_angle = (((uint16_t)buf[0] << 8) | buf[1]) & 0x0FFF;
  } else {
    *raw_angle = 0;
  }
  return status;
}

HAL_StatusTypeDef AS5600_GetScaledAngle(I2C_HandleTypeDef *hi2c,
                                        uint16_t *scaled_angle) {
  uint8_t buf[2];
  HAL_StatusTypeDef status = _AS5600_ReadReg(hi2c, AS5600_REG_ANGLE_H, buf, 2);

  if (status == HAL_OK) {
    *scaled_angle = ((uint16_t)buf[0] << 8) | buf[1] & 0x0FFF;
  } else {
    *scaled_angle = 0;
  }
  return status;
}

float AS5600_RawAngleToDegrees(uint16_t raw_angle) {
  // 将 0-4095 的值映射到 0-360 度
  return (float)raw_angle * AS5600_RAW_TO_DEG;
}

float AS5600_RawAngleToRadians(uint16_t raw_angle) {
  // 将 0-4095 的值映射到 0-2PI 弧度
  return (float)raw_angle * AS5600_RAW_TO_RAD;
}

HAL_StatusTypeDef AS5600_GetStatus(I2C_HandleTypeDef *hi2c,
                                   uint8_t *status_reg) {
  // 状态寄存器只包含 3 位有效信息 (MH, ML, MD)
  return _AS5600_ReadReg(hi2c, AS5600_REG_STATUS, status_reg, 1);
}
